// ========================= ethercatWindows.c =========================
// Include SOEM if available. Keep fallback stubs to compile without SOEM.

#include "ethercatWindows.h"
#include "ethercat.h"
#include <string.h>
#include <stdio.h>
#include <math.h>

#ifdef _WIN32

static CRITICAL_SECTION g_lock;
static HANDLE g_thread = NULL;
#else
#include <pthread.h>
#include <time.h>
static pthread_mutex_t g_lock = PTHREAD_MUTEX_INITIALIZER;
static pthread_t g_thread;
#endif

#define CYCTIME_MS 12
#define MODE_CSP 8
#define MODE_PP 1
int64 toff = 0;
int expectedWKC = 0;
volatile int wkc = 0;
boolean inOP = false;
char IOmap[4096];

uint16_t di_val;
int do_val = 0;

char *Ifname = "\\Device\\NPF_{5FF298A4-DED9-447B-A9F3-404A6958B211}";
int Port = 8080;

enum
{
    STATE_RESET,
    STATE_INIT,
    STATE_PREREADY,
    STATE_READY,
    STATE_PREENABLE,
    STATE_ENABLE,
    STATE_DISABLE
};

int states[50];

typedef struct _stswrd_bits_t
{
    uint8_t rtso : 1; // BIT0: ready to switch on
    uint8_t so : 1;   // BIT1: switched on
    uint8_t oe : 1;   // BIT2: operation enabled
    uint8_t f : 1;    // BIT3: fault
    uint8_t ve : 1;   // BIT4:  voltage enabled
    uint8_t qs : 1;   // BIT5: quick stop
    uint8_t sod : 1;  // BIT6: switch on disabled
    uint8_t w : 1;    // BIT7: warning
    uint8_t ms1 : 1;  // BIT8: manufacturer-specific1
    uint8_t rm : 1;   // BIT9: remote
    uint8_t tr : 1;   // BIT10: target reached
    uint8_t ila : 1;  // BIT11:  internal limit active
    uint8_t oms1 : 1; // BIT12: oms1 pp-Set-point acknowledge; hm-Homing attained;
                      //         ip-ip  mode active;csp-Target position ignored;
                      //         csv-Target velocity ignored;cst-Target torque ignored;
    uint8_t oms2 : 1; // BIT13: oms1 pp-Following error; hm-Homing error; csp-Following error;
    uint8_t ms2 : 1;  // BIT14: manufacturer-specific2
    uint8_t ms3 : 1;  // BIT15: manufacturer-specific3
} stswrd_bits_t;

typedef union _stswrd_t
{
    uint16_t all;
    stswrd_bits_t bit;
} stswrd_t;

static int drive_write8(uint16 slave, uint16 index, uint8 subindex, uint8 value)
{
    return ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);
}

static int drive_write16(uint16 slave, uint16 index, uint8 subindex, uint16 value)
{
    return ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);
}

static int drive_write32(uint16 slave, uint16 index, uint8 subindex, int32 value)
{
    return ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);
}

#ifdef _MSC_VER // Windows MSVC
#define PACKED
#pragma pack(push, 1)
#else // GCC/Clang (Linux)
#define PACKED __attribute__((packed))
#endif

typedef struct PACKED
{
    uint16_t data;
} Drive_Inputs_IO;

typedef struct PACKED
{
    uint16_t data;
} Drive_Outputs_IO;

// typedef struct PACKED
// {
//     uint16_t Controlword;
//     uint16_t Return_Request;
//     uint32_t Out_test;

//     uint16_t DO;

// } Drive_Outputs_IO;

// typedef struct PACKED
// {
//     uint16_t SM_states;
//     uint32_t IO_Inputs;
//     uint16_t Request_Code;
//     uint16_t Error_Code;
//     uint16_t Master_Modes;
//     uint32_t In_test;

//     uint16_t DI;

// } Drive_Inputs_IO;

typedef struct PACKED
{
    uint16_t Controlword;
    int32_t TargetPos;
    uint8_t ModeOp;
} Servo_Outputs_CSP;

typedef struct PACKED
{
    uint16_t Controlword;
    int32_t TargetPos;
    uint32_t TargetVel;
    uint32_t ACC;
    uint32_t DCC;
    uint8_t ModeOp;
} Servo_Outputs_PP;

typedef struct PACKED
{
    uint16_t Statusword;
    int32_t ActualPos;
    int32_t ActualVel;
    int16_t ActualTor;
    uint8_t ModeOp;
} Servo_Inputs;

#ifdef _MSC_VER
#pragma pack(pop)
#endif

int servo_setup_CSP(uint16 slave)
{
    int wkc = 0;

    printf("servo_setup_CSP \n");

    wkc += drive_write16(slave, 0x1C12, 0, 0);
    wkc += drive_write16(slave, 0x1C13, 0, 0);

    wkc += drive_write16(slave, 0x1A00, 0, 0);
    wkc += drive_write32(slave, 0x1A00, 1, 0x60410010); // Statusword
    wkc += drive_write32(slave, 0x1A00, 2, 0x60640020); // Position actual value
    wkc += drive_write32(slave, 0x1A00, 3, 0x606C0020); // Velocity actual value
    wkc += drive_write32(slave, 0x1A00, 4, 0x60770010); // Torque actual value
    wkc += drive_write32(slave, 0x1A00, 5, 0x60610008); // Modes of operation display
    wkc += drive_write8(slave, 0x1A00, 0, 5);

    wkc += drive_write8(slave, 0x1600, 0, 0);
    wkc += drive_write32(slave, 0x1600, 1, 0x60400010); // Controlword
    wkc += drive_write32(slave, 0x1600, 2, 0x607A0020); // Target position
    // wkc += drive_write32(slave, 0x1600, 3, 0x60810020); // Target velocity
    // wkc += drive_write32(slave, 0x1600, 4, 0x60830020); // ACC
    // wkc += drive_write32(slave, 0x1600, 5, 0x60840020); // DCC
    wkc += drive_write32(slave, 0x1600, 6 - 3, 0x60600008); // Modes of operation display
    wkc += drive_write8(slave, 0x1600, 0, 6 - 3);

    wkc += drive_write16(slave, 0x1C12, 1, 0x1600);
    wkc += drive_write8(slave, 0x1C12, 0, 1);

    wkc += drive_write16(slave, 0x1C13, 1, 0x1A00);
    wkc += drive_write8(slave, 0x1C13, 0, 1);

    strncpy(ec_slave[slave].name, "Drive", EC_MAXNAME);

    if (wkc != 21 - 3)
    {
        printf("Drive %d setup failed\nwkc: %d\n", slave, wkc);
        return -1;
    }
    else
        printf("Drive %d setup succeed.\n", slave);

    return 0;
}

int servo_setup_PP(uint16 slave)
{
    int wkc = 0;

    printf("servo_setup_PP \n");

    wkc += drive_write16(slave, 0x1C12, 0, 0);
    wkc += drive_write16(slave, 0x1C13, 0, 0);

    wkc += drive_write16(slave, 0x1A00, 0, 0);
    wkc += drive_write32(slave, 0x1A00, 1, 0x60410010); // Statusword
    wkc += drive_write32(slave, 0x1A00, 2, 0x60640020); // Position actual value
    wkc += drive_write32(slave, 0x1A00, 3, 0x606C0020); // Velocity actual value
    wkc += drive_write32(slave, 0x1A00, 4, 0x60770010); // Torque actual value
    wkc += drive_write32(slave, 0x1A00, 5, 0x60610008); // Modes of operation display
    wkc += drive_write8(slave, 0x1A00, 0, 5);

    wkc += drive_write8(slave, 0x1600, 0, 0);
    wkc += drive_write32(slave, 0x1600, 1, 0x60400010); // Controlword
    wkc += drive_write32(slave, 0x1600, 2, 0x607A0020); // Target position
    wkc += drive_write32(slave, 0x1600, 3, 0x60810020); // Target velocity
    wkc += drive_write32(slave, 0x1600, 4, 0x60830020); // ACC
    wkc += drive_write32(slave, 0x1600, 5, 0x60840020); // DCC
    wkc += drive_write32(slave, 0x1600, 6, 0x60600008); // Modes of operation display
    wkc += drive_write8(slave, 0x1600, 0, 6);

    wkc += drive_write16(slave, 0x1C12, 1, 0x1600);
    wkc += drive_write8(slave, 0x1C12, 0, 1);

    wkc += drive_write16(slave, 0x1C13, 1, 0x1A00);
    wkc += drive_write8(slave, 0x1C13, 0, 1);

    strncpy(ec_slave[slave].name, "Drive", EC_MAXNAME);

    if (wkc != 21)
    {
        printf("Drive %d setup failed\nwkc: %d\n", slave, wkc);
        return -1;
    }
    else
        printf("Drive %d setup succeed.\n", slave);

    return 0;
}

int SOEM_INIT()
{

    if (!ec_init(Ifname))
    {
        printf("EtherCAT initialization failed.\n");
        return -1;
    }

    printf("ec_init on %s succeeded.\n", Ifname);
    /* find and auto-config slaves */
    if (ec_config_init(FALSE) > 0)
    {
        ecx_context.manualstatechange = 1;
        printf("%d slaves found and configured.\n", ec_slavecount);
        /* wait for all slaves to reach SAFE_OP state */

        for (int slave_ix = 1; slave_ix <= ec_slavecount; slave_ix++)
        {
            ec_slavet *slave = &ec_slave[slave_ix];
            if (Devices[slave_ix - 1].type == DEV_SERVO_CSP)
            {
                slave->PO2SOconfig = servo_setup_CSP;
            }
            else if (Devices[slave_ix - 1].type == DEV_SERVO_PP)
            {
                slave->PO2SOconfig = servo_setup_PP;
            }
            else if (Devices[slave_ix - 1].type == DEV_IO)
            {
                // NO SET UP
            }
        }

        ec_config_map(&IOmap); // 此处调用drive_setup函数，进行PDO映射表设置

        // setup dc for devices
        for (int slave_ix = 1; slave_ix <= ec_slavecount; slave_ix++)
        {
            ec_dcsync0(slave_ix, TRUE, CYCTIME_MS * 1000000U, CYCTIME_MS * 1000000U / 4);
        }
        ec_configdc(); // 设置同步时钟，该函数必须在设置pdo映射之后；

        printf("Slaves mapped, state to SAFE_OP.\n");
        ec_slave[0].state = EC_STATE_SAFE_OP;
        /* send one valid process data to make outputs in slaves happy*/

        /* request OP state for all slaves */
        ec_writestate(0);
        ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);

        /* read indevidual slave state and store in ec_slave[] */

        ec_readstate();
        for (int cnt = 1; cnt <= ec_slavecount; cnt++)
        {
            printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
                   cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
                   ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
        }
        expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
        printf("Calculated workcounter %d\n", expectedWKC);

        printf("Request operational state for all slaves\n");
        /* activate cyclic process data */
        /* wait for all slaves to reach OP state */
        ec_slave[0].state = EC_STATE_OPERATIONAL;
        /* request OP state for all slaves */
        ec_writestate(0);

        /* 对PDO进行初始化 */
        for (int i = 0; i < ec_slavecount; i++)
        {
            if (Devices[i].type == DEV_SERVO_PP || Devices[i].type == DEV_SERVO_CSP)
            {
                states[i + 1] = STATE_RESET;
            }
        }
    }
    else
    {
        printf("No slaves found!\n");
        ec_close();
        return -1;
    }

    return 1;
}

static volatile int g_run = 0;

Device Devices[MAX_DEVICES];
int DeviceCount = 0;

// Command mailbox (very light; EtherCAT thread picks and clears)
typedef struct
{
    int valid;
    int idx;
    int bit;
    int value;
} CmdSetDO;
typedef struct
{
    int valid;
    int idx;
    int idx2; //辅助伺服,用于桁架模式,需要双伺服联动的场景. 必须保证两个伺服的编码器值一致
    int32_t target, vel, acc, dec;
} CmdMove;
typedef struct
{
    int valid;
    int idx;
} CmdStop;

static CmdSetDO g_cmd_do;
static CmdMove g_cmd_move;
static CmdStop g_cmd_stop;

// 读取整个文件为字符串
static char *read_file(const char *filename)
{
    FILE *f = fopen(filename, "rb");
    if (!f)
        return NULL;
    fseek(f, 0, SEEK_END);
    long len = ftell(f);
    fseek(f, 0, SEEK_SET);
    char *data = (char *)malloc(len + 1);
    if (!data)
    {
        fclose(f);
        return NULL;
    }
    fread(data, 1, len, f);
    data[len] = '\0';
    fclose(f);
    return data;
}

// 从 config.json 填充 Devices[]
static int load_config(const char *filename)
{
    char *text = read_file(filename);
    if (!text)
    {
        printf("Cannot open %s\n", filename);
        return -1;
    }

    cJSON *json = cJSON_Parse(text);
    free(text);
    if (!json)
    {
        printf("Invalid JSON\n");
        return -1;
    }

    // 解析 port
    cJSON *port = cJSON_GetObjectItem(json, "port");
    if (cJSON_IsNumber(port))
    {
        Port = port->valueint;
    }
    else
    {
        printf("No 'port' in config, using default 7070\n");
        Port = 7070;
    }

    // 解析 netcard
    cJSON *netcard = cJSON_GetObjectItem(json, "netcard");
    if (cJSON_IsString(netcard))
    {
        // strncpy(IfName, netcard->valuestring, sizeof(IfName) - 1);

        Ifname = _strdup(netcard->valuestring);
    }
    else
    {
        printf("No 'netcard' in config, using default empty\n");
    }

    cJSON *devices = cJSON_GetObjectItem(json, "devices");
    if (!cJSON_IsArray(devices))
    {
        printf("No 'devices' array in config\n");
        cJSON_Delete(json);
        return -1;
    }

    DeviceCount = 0;
    cJSON *dev;
    for (dev = devices->child; dev; dev = dev->next)
    {
        if (DeviceCount >= MAX_DEVICES)
            break;

        cJSON *type = cJSON_GetObjectItem(dev, "type");
        cJSON *name = cJSON_GetObjectItem(dev, "name");

        if (!cJSON_IsString(type) || !cJSON_IsString(name))
            continue;

        Device *d = &Devices[DeviceCount];
        d->index = DeviceCount;
        strncpy(d->name, name->valuestring, sizeof(d->name));

        if (_stricmp(type->valuestring, "servo_PP") == 0)
        {
            d->type = DEV_SERVO_PP;
            d->u.servo.position = 0;
            d->u.servo.velocity = 0;
            d->u.servo.torque = 0;
            d->u.servo.target_position = 0;
            d->u.servo.target_velocity = 0;
            d->u.servo.status = 0;
            d->u.servo.error = 0;
            d->u.servo.running = 0;

            printf("servo_PP at %d \n", DeviceCount);
        }
        else if (_stricmp(type->valuestring, "io") == 0)
        {
            d->type = DEV_IO;
            for (int b = 0; b < IO_BITS; b++)
            {
                d->u.io.DO[b] = 0;
                d->u.io.DI[b] = 0;
            }

            printf("io at %d \n", DeviceCount);
        }
        else if (_stricmp(type->valuestring, "servo_CSP") == 0)
        {
            d->type = DEV_SERVO_CSP;
            d->u.servo.position = 0;
            d->u.servo.velocity = 0;
            d->u.servo.torque = 0;
            d->u.servo.target_position = 0;
            d->u.servo.target_velocity = 0;
            d->u.servo.status = 0;
            d->u.servo.error = 0;
            d->u.servo.running = 0;

            printf("servo_CSP at %d \n", DeviceCount);
        }
        else
        {
            continue;
        }

        DeviceCount++;
    }

    cJSON_Delete(json);
    printf(" config loaded got %d Devices \n", DeviceCount);
    return 0;
}

static void lock()
{
#ifdef _WIN32
    EnterCriticalSection(&g_lock);
#else
    pthread_mutex_lock(&g_lock);
#endif
}
static void unlock()
{
#ifdef _WIN32
    LeaveCriticalSection(&g_lock);
#else
    pthread_mutex_unlock(&g_lock);
#endif
}

static void sleep_ms_ec(int ms)
{
#ifdef _WIN32
    Sleep(ms);
#else
    struct timespec ts = {ms / 1000, (ms % 1000) * 1000000L};
    nanosleep(&ts, NULL);
#endif
}

// ---- public API ----
int ethercatw_get_snapshot(Device *out, int max_out)
{
    int n = 0;
    lock();
    n = (DeviceCount < max_out) ? DeviceCount : max_out;
    memcpy(out, Devices, sizeof(Device) * n);
    unlock();
    return n;
}

int ethercatw_set_do(int slaveIndex, int bit, int value)
{
    lock();
    g_cmd_do.valid = 1;
    g_cmd_do.idx = slaveIndex;
    g_cmd_do.bit = bit;
    g_cmd_do.value = value;
    Devices[slaveIndex].u.io.DO[bit] = value; // 立即更新状态
    unlock();
    return 0;
}
int ethercatw_move_axis(int slaveIndex, int32_t target, int32_t vel, int32_t acc, int32_t dec)
{
    lock();
    g_cmd_move.valid = 1;
    g_cmd_move.idx = slaveIndex;
    g_cmd_move.target = target;
    g_cmd_move.vel = vel;
    g_cmd_move.acc = acc;
    g_cmd_move.dec = dec;
    unlock();
    return 0;
}

int ethercatw_move_2axes(int slaveIndex,int slaveIndex2, int32_t target, int32_t vel, int32_t acc, int32_t dec)
{
    lock();
    g_cmd_move.valid = 2;
    g_cmd_move.idx = slaveIndex;
    g_cmd_move.idx2 = slaveIndex2;
    g_cmd_move.target = target;
    g_cmd_move.vel = vel;
    g_cmd_move.acc = acc;
    g_cmd_move.dec = dec;
    unlock();
    return 0;
}

int ethercatw_stop_axis(int slaveIndex)
{
    lock();
    g_cmd_stop.valid = 1;
    g_cmd_stop.idx = slaveIndex;
    unlock();
    return 0;
}

void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime)
{
    static int64 integral = 0;
    int64 delta;
    /* set linux sync point 50us later than DC sync, just as example */
    delta = (reftime - 50000) % cycletime;
    if (delta > (cycletime / 2))
    {
        delta = delta - cycletime;
    }
    if (delta > 0)
    {
        integral++;
    }
    if (delta < 0)
    {
        integral--;
    }
    *offsettime = -(delta / 100) - (integral / 20);
}

#ifdef _WIN32
static DWORD WINAPI ec_thread_fn(LPVOID arg)
#else
static void *ec_thread_fn(void *arg)
#endif
{
    (void)arg;
    int ccnt = 0;
    int cnt = 0;
    int i = 0;
    Servo_Inputs *iptr;
    Servo_Outputs_CSP *optr_csp;
    Servo_Outputs_PP *optr;

    Drive_Inputs_IO *DIptr;
    Drive_Outputs_IO *DOptr;
    int state;
    union _stswrd_t status;

    SOEM_INIT();
    int dire = 0;

    double Ts = CYCTIME_MS / 1000.0; // 周期 (秒)
    double delta_v;

    int32_t remaining;
    int32_t direction;

    // 计算减速距离
    double dec_distance;
    double delta_p;
    int current_vel;

    while (g_run)
    {

        osal_usleep(CYCTIME_MS * 1000);
        wkc = ec_receive_processdata(EC_TIMEOUTRET);

        if (wkc == expectedWKC && inOP == false)
        {
            inOP = true;
            printf("EtherCAT in OPERATIONAL state.\n");
        }
        if (inOP == true)
        {

            for (i = 0; i < ec_slavecount; i++)
            {
                if (Devices[i].type == DEV_SERVO_PP)
                {
                    optr = (Servo_Outputs_PP *)ec_slave[i + 1].outputs;
                    iptr = (Servo_Inputs *)ec_slave[i + 1].inputs;
                    status.all = iptr->Statusword;
                    state = states[i];

                    Devices[i].u.servo.position = iptr->ActualPos;
                    Devices[i].u.servo.velocity = iptr->ActualVel;
                    Devices[i].u.servo.torque = iptr->ActualTor;
                    Devices[i].u.servo.status = iptr->Statusword;
                    Devices[i].u.servo.error = 0; // TODO: vendor-specific error code

                    switch (state)
                    {
                    case STATE_RESET:
                        optr->Controlword = 6;
                        optr->ModeOp = 0;

                        states[i] = STATE_INIT;
                        break;
                    case STATE_INIT /* 初始化驱动器,清除错误 */:
                        optr->Controlword = 134;

                        if (status.bit.sod || status.bit.so || status.bit.rtso)
                        {
                            optr->ModeOp = MODE_PP;
                            states[i] = STATE_PREREADY;
                        }
                        break;
                    case STATE_PREREADY:
                        optr->Controlword = 6;
                        optr->ModeOp = MODE_PP;

                        if (status.bit.rtso && status.bit.qs)
                        {
                            ccnt++;
                            if (ccnt > 200 * ec_slavecount)
                            {
                                if (i + 1 == ec_slavecount)
                                {
                                    ccnt = 0;
                                }

                                states[i] = STATE_READY;
                            }
                        }
                        break;
                    case STATE_READY /* 系统转为准备使能状态 */:
                        optr->Controlword = 7;
                        optr->ModeOp = MODE_PP;

                        if (status.bit.rtso && status.bit.qs && status.bit.so)
                        {
                            ccnt++;
                            if (ccnt > 220 * ec_slavecount)
                            {
                                if (i + 1 == ec_slavecount)
                                {
                                    ccnt = 0;
                                }

                                states[i] = STATE_PREENABLE;
                            }
                        }
                        break;
                    case STATE_PREENABLE /* 驱动器使能 */:
                        optr->Controlword = 0xF;
                        optr->ModeOp = MODE_PP;
                        optr->ACC = 10000;
                        optr->DCC = 10000;
                        if (status.bit.rtso && status.bit.qs && status.bit.so && status.bit.oe)
                        {
                            states[i] = STATE_ENABLE;
                        }
                        break;
                    case STATE_ENABLE /* 驱动器运动 */:
                        // optr->Controlword = 31;
                        optr->ModeOp = MODE_PP;

                        if (g_cmd_move.valid == 1 && g_cmd_move.idx == i)
                        {
                            optr->TargetPos = g_cmd_move.target;
                            optr->TargetVel = g_cmd_move.vel;
                            optr->ACC = g_cmd_move.acc;
                            optr->DCC = g_cmd_move.dec;
                            optr->Controlword = 0xf;
                            g_cmd_move.valid = 2;
                            Devices[i].u.servo.running = 1;
                            Devices[i].u.servo.target_position = g_cmd_move.target;
                            Devices[i].u.servo.target_velocity = g_cmd_move.vel;
                            printf("%d Devices[i].u.servo.running = 1 \n", i);
                        }
                        else if (g_cmd_move.valid == 2 && g_cmd_move.idx == i)
                        {
                            Devices[i].u.servo.stepcnt++;
                            if (Devices[i].u.servo.stepcnt > 20)
                            {
                                optr->Controlword = 0x1F; // 运动
                                g_cmd_move.valid = 0;
                                printf("%d optr->Controlword = 0x1F \n", i);
                                Devices[i].u.servo.stepcnt = 0;
                            }
                        }

                        else if (g_cmd_stop.valid == 1 && g_cmd_stop.idx == i)
                        {
                            g_cmd_stop.valid = 0;
                            g_cmd_move.valid = 0;
                            optr->Controlword = 0x1F | 0x0100;
                            Devices[i].u.servo.running = 0;
                            printf("%d optr->Controlword = %d \n", i, optr->Controlword);
                        }

                        break;
                    case STATE_DISABLE:

                        optr->ModeOp = 0;
                        // optr->TargetVel = 0;
                        optr->Controlword = 6;

                        break;
                    default:
                        break;
                    }
                }
                else if (Devices[i].type == DEV_SERVO_CSP)
                {
                    optr_csp = (Servo_Outputs_CSP *)ec_slave[i + 1].outputs;
                    iptr = (Servo_Inputs *)ec_slave[i + 1].inputs;
                    status.all = iptr->Statusword;
                    state = states[i];

                    /*  printf(" %d:CW:%d,SW:%d, Md: %d, p:%d, v:%d, t:%d \n", i + 1, optr->Controlword, iptr->Statusword, iptr->ModeOp,
                      iptr->ActualPos,
                      iptr->ActualVel,
                      iptr->ActualTor);*/
                    Devices[i].u.servo.position = iptr->ActualPos;
                    Devices[i].u.servo.velocity = iptr->ActualVel;
                    Devices[i].u.servo.torque = iptr->ActualTor;
                    Devices[i].u.servo.status = iptr->Statusword;
                    Devices[i].u.servo.error = 0; // TODO: vendor-specific error code

                    switch (state)
                    {
                    case STATE_RESET:
                        optr_csp->Controlword = 6;
                        optr_csp->ModeOp = MODE_CSP;

                        states[i] = STATE_INIT;
                        break;
                    case STATE_INIT /* 初始化驱动器,清除错误 */:
                        optr_csp->Controlword = 134;
                        optr_csp->TargetPos = iptr->ActualPos;

                        if (status.bit.sod || status.bit.so || status.bit.rtso)
                        {
                            optr_csp->ModeOp = MODE_CSP;
                            states[i] = STATE_PREREADY;
                        }
                        break;
                    case STATE_PREREADY:
                        optr_csp->Controlword = 6;
                        optr_csp->ModeOp = MODE_CSP;
                        optr_csp->TargetPos = iptr->ActualPos;

                        if (status.bit.rtso && status.bit.qs)
                        {
                            ccnt++;
                            if (ccnt > 200 * ec_slavecount)
                            {
                                if (i + 1 == ec_slavecount)
                                {
                                    ccnt = 0;
                                }

                                states[i] = STATE_READY;
                            }
                        }
                        break;
                    case STATE_READY /* 系统转为准备使能状态 */:
                        optr_csp->Controlword = 7;
                        optr_csp->ModeOp = MODE_CSP;
                        optr_csp->TargetPos = iptr->ActualPos;

                        if (status.bit.rtso && status.bit.qs && status.bit.so)
                        {
                            ccnt++;
                            if (ccnt > 220 * ec_slavecount)
                            {
                                if (i + 1 == ec_slavecount)
                                {
                                    ccnt = 0;
                                }

                                states[i] = STATE_PREENABLE;
                            }
                        }
                        break;
                    case STATE_PREENABLE /* 驱动器使能 */:
                        optr_csp->Controlword = 15;
                        optr_csp->ModeOp = MODE_CSP;
                        optr_csp->TargetPos = iptr->ActualPos;
                        if (status.bit.rtso && status.bit.qs && status.bit.so && status.bit.oe)
                        {
                            states[i] = STATE_ENABLE;
                        }
                        break;
                    case STATE_ENABLE /* 驱动器运动 */:
                        // optr->Controlword = 31;
                        optr_csp->ModeOp = MODE_CSP;

                        if (g_cmd_move.valid > 0 && ((g_cmd_move.idx == i) || (g_cmd_move.idx2 == i)))
                        {
                            g_cmd_move.valid -- ;
                            Devices[i].u.servo.running = 1;
                            Devices[i].u.servo.target_position = g_cmd_move.target;
                            Devices[i].u.servo.target_velocity = g_cmd_move.vel;
                            
                            Devices[i].u.servo.dec = g_cmd_move.dec;
                            Devices[i].u.servo.acc = g_cmd_move.acc;
                            Devices[i].u.servo.traj_vel = 0; // 内部轨迹速度（控制器计算的）
                            printf("%d Devices[i].u.servo.running = 1 MODE_CSP target_velocity \n", i, Devices[i].u.servo.target_velocity);
                        }

                        if (g_cmd_stop.valid == 1 && g_cmd_move.idx == i)
                        {
                            g_cmd_stop.valid = 0;
                            Devices[i].u.servo.running = 0;
                            Devices[i].u.servo.traj_vel = 0;
                            optr_csp->TargetPos = Devices[i].u.servo.position;
                            Devices[i].u.servo.target_position = Devices[i].u.servo.position;
                            break;                       
                        }

                        if (Devices[i].u.servo.running)
                        {
                            // 剩余距离
                            remaining = Devices[i].u.servo.target_position - Devices[i].u.servo.position;
                            direction = 0;
                            if (remaining > 2000)
                                direction = 1;
                            else if (remaining < -2000)
                                direction = -1;
                            else{
                                Devices[i].u.servo.running = 0;
                                Devices[i].u.servo.traj_vel = 0;
                                optr_csp->TargetPos = Devices[i].u.servo.position;
                                Devices[i].u.servo.target_position = Devices[i].u.servo.position;
                                printf("%d reach target pos %d \n", i, Devices[i].u.servo.target_position);
                                break;
                            }

                            // 计算减速距离
                            dec_distance = ( ((uint64_t)Devices[i].u.servo.target_velocity) * ((uint64_t)Devices[i].u.servo.target_velocity) / (2.0 * Devices[i].u.servo.dec) );
                            current_vel = Devices[i].u.servo.traj_vel;

                            // 判断当前阶段
                            if (abs(remaining) > dec_distance)
                            {
                                // 加速或匀速
                                if (current_vel < g_cmd_move.vel)
                                {
                                    // 🔹 加速阶段
                                    delta_v = g_cmd_move.acc * Ts;
                                    current_vel += delta_v;
                                    if (current_vel > g_cmd_move.vel)
                                        current_vel = g_cmd_move.vel;
                                    // printf("acc %d, tv %d,  v %d, r %d, dis %f \n",Devices[i].u.servo.dec,Devices[i].u.servo.target_velocity,current_vel,remaining,dec_distance);

                                }
                                else
                                {
                                    // 🔹 匀速阶段
                                    current_vel = g_cmd_move.vel;

                                    // printf(" r %d, dis %.2f \n",remaining,dec_distance);
                                }
                            }
                            else
                            {
                                // 🔹 减速阶段
                                delta_v = g_cmd_move.dec * Ts;
                                current_vel -= delta_v;
                                if (current_vel < 0)
                                    current_vel = 0;
                                // printf("dec %d \n",current_vel);
                            }

                            Devices[i].u.servo.traj_vel = current_vel;
                            // 计算本周期位移
                            delta_p = current_vel * Ts;

                            // 下发位置
                            optr_csp->TargetPos = iptr->ActualPos + (int32_t)(delta_p * direction);
                        }

                        break;
                    case STATE_DISABLE:

                        optr_csp->ModeOp = 0;
                        optr_csp->Controlword = 6;

                        break;
                    default:
                        break;
                    }
                }
                else if (Devices[i].type == DEV_IO)
                {
                    DIptr = (Drive_Inputs_IO *)ec_slave[i + 1].inputs;
                    di_val = DIptr->data;
                    for (cnt = 0; cnt < 8; cnt++)
                    {
                        Devices[i].u.io.DI[cnt] = (di_val >> cnt) & 0x01;
                    }
                    DOptr = (Drive_Outputs_IO *)ec_slave[i + 1].outputs;

                    do_val = 0;
                    // 把数组的每一位打包成 16 位整数
                    for (cnt = 0; cnt < 8; cnt++)
                    {
                        if (Devices[i].u.io.DO[cnt] == 1)
                        {
                            do_val += pow(2, cnt);
                        }
                    }

                    DOptr->data = do_val;
                }
            }
        }

        ec_send_processdata();
        // unlock();
    }

#ifdef _WIN32
    return 0;
#else
    return NULL;
#endif
}

int ethercatw_start()
{
#ifdef _WIN32
    InitializeCriticalSection(&g_lock);
#endif

    lock();
    DeviceCount = 0;
    unlock();

    // 🔥 从 config.json 加载设备
    if (load_config("config.json") != 0)
    {
        printf("Failed to load config.json\n");
        return -1;
    }

    g_run = 1;
#ifdef _WIN32
    g_thread = CreateThread(NULL, 0, ec_thread_fn, NULL, 0, NULL);
    if (!g_thread)
    {
        g_run = 0;
        return -1;
    }
#else
    if (pthread_create(&g_thread, NULL, ec_thread_fn, NULL) != 0)
    {
        g_run = 0;
        return -1;
    }
#endif
    return 0;
}

void ethercatw_stop(void)
{
    g_run = 0;
#ifdef _WIN32
    if (g_thread)
    {
        WaitForSingleObject(g_thread, INFINITE);
        CloseHandle(g_thread);
        g_thread = NULL;
    }
    DeleteCriticalSection(&g_lock);
#else
    pthread_join(g_thread, NULL);
#endif
}